#include "plugins/action/data_prerocess_action.hpp"

DataPreProcessAction::DataPreProcessAction(const std::string& name, const BT::NodeConfig& config)
    : BT::SyncActionNode(name, config) {        
    sensorData sensor_data_raw = {};
    sensorData sensor_data_filtered = {};
    
    // 初始化 ROS2 发布者
    ros_node_ptr_ = std::make_shared<estMsgPublisher>("data_preprocess_publisher", this);
    msg_publisher = ros_node_ptr_->create_publisher<std_msgs::msg::Float32MultiArray>("filtered_sensor_data", 10);
    ros_node_ptr_ = std::make_shared<estMsgPublisher>("data_preprocess_node", this);
    raw_publisher = ros_node_ptr_->create_publisher<std_msgs::msg::Float32MultiArray>("raw_sensor_data", 10);

}

//增量更新方法计算均值，只维护均值队列（如果不需要原始数据）精度累积误差;第二种方法: 滑动窗口均值,维护原始数据的队列,均值计算更加准确
void DataPreProcessAction::new_value_update_mean(float new_value, std::deque<float>& mean_history, size_t window_size){
        if (mean_history.empty()) {
            // 初始情况：第一个均值就是新值本身
            mean_history.push_back(new_value);
            return;
        }
        // 获取当前均值
        float current_mean = mean_history.back();
        size_t current_size = (mean_history.size() < window_size) ? mean_history.size() : window_size;        
        // 增量更新公式: new_mean = (current_mean * n + new_value) / (n + 1)
        float new_mean = (current_mean * current_size + new_value) / (current_size + 1);
        // 添加新均值
        mean_history.push_back(new_mean);
        
        // 保持队列长度
        if (mean_history.size() > window_size) {
            mean_history.pop_front();
        }
}

float DataPreProcessAction::iir_filter(float x) {
    // 计算 y[n]
    float y = b[0] * x 
             + b[1] * x_history[0] 
             + b[2] * x_history[1]
             - a[1] * y_history[0] 
             - a[2] * y_history[1];

    // 更新历史数据

    x_history[1] = x_history[0];
    x_history[0] = x;

    y_history[1] = y_history[0];
    y_history[0] = y;
    return y;
}

float DataPreProcessAction::move_averge_filter(float x) {
    int length = sizeof(buffer) / sizeof(buffer[0]);
    buffer[index] = x;       // 保存当前输入
    index = (index + 1) % length; // 环形缓冲
    float y = 0.0f;
    for (int i = 0; i < length; i++) {
        y += buffer[i];
    }
    y *= 0.166667f;  // 乘以系数
    return y;
}




BT::NodeStatus DataPreProcessAction:: tick() {
    // 读取输入端口（取消注释并处理错误）
    if (!getInput<sensorData>("raw_sensor_input", sensor_data_raw)) {
        throw BT::RuntimeError("Missing required input [raw_sensor_input]");
    }
    //   std::cout << "Current data: imu_euler=" << sensor_data_raw.imu_euler 
    //             << ", imu_gyr=" << sensor_data_raw.imu_gyr 
    //             << ", tof_distance=" << sensor_data_raw.tof_distance << std::endl;

    
    float tmp_sensor_tof_distance = iir_filter(sensor_data_raw.tof_distance);
    new_value_update_mean(tmp_sensor_tof_distance, mean_tof_history);

    sensor_data_filtered.tof_distance = tmp_sensor_tof_distance - mean_tof_history.back();
    sensor_data_filtered.imu_gyr = move_averge_filter(sensor_data_raw.imu_gyr);
    

    setOutput("filtered_sensor_output", sensor_data_filtered);

    

    // 发布数据到 ROS2 话题
    raw_data.data.clear();
    raw_data.data.push_back(static_cast<float>(sensor_data_raw.imu_gyr));
    raw_data.data.push_back(static_cast<float>(sensor_data_raw.tof_distance));
    raw_publisher->publish(raw_data);

    message.data.clear();
    // message.data.push_back(sensor_data_filtered.imu_euler);
    message.data.push_back(sensor_data_filtered.imu_gyr);
    message.data.push_back(sensor_data_filtered.tof_distance);      
    msg_publisher->publish(message);


    // std::cout << "dataPreProcess: Filtered data generated and published" << std::endl;
    return BT::NodeStatus::SUCCESS;
}




// estMsgPublisher 构造函数实现
DataPreProcessAction::estMsgPublisher::estMsgPublisher(const std::string& node_name, DataPreProcessAction* pClass)
    : Node(node_name), pTofFusion(pClass) {
    // 确保 ROS2 上下文已初始化
    if (!rclcpp::ok()) {
        rclcpp::init(0, nullptr);
    }
}

// estMsgPublisher 析构函数实现
DataPreProcessAction::estMsgPublisher::~estMsgPublisher() {
    // 析构函数逻辑
}

#include "behaviortree_cpp/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  std::cout << "Registering nodes...: DataPreProcessAction" << std::endl;//在宏内添加调试信息或条件判断
  factory.registerNodeType<DataPreProcessAction>("DataPreProcessAction");
}